clear all;close all;clc;

k = 20;  % 输入聚类组数
count = 1000;  % 定义最大循环次数

minpts = 50;
epsilon = 0.5;

%%SR71雷达数据保存路径
% E:\Documents\MATLAB\radar_data_9_3\雷达移动\SR71\data2
% E:\Documents\MATLAB\radar_data_9_3\雷达固定，人动\SR71\data2
% E:\Documents\MATLAB\radar_data_9_3\悬顶\SR71\data1
%%SR73F雷达数据保存路径
% E:\Documents\MATLAB\radar_data_9_3\雷达固定，人动\SR73F\data1
% E:\Documents\MATLAB\radar_data_9_3\雷达移动\SR73\data2
% E:\Documents\MATLAB\radar_data_9_3\悬顶\SR73F\data1
% E:\Documents\MATLAB\radar_data_9_3\振动台\SR73F
%%
folder = 'E:\Documents\MATLAB\radar_data_9_11\SR73窄通道斜45度运动';
fileList = dir(fullfile(folder, '*.txt'));
filenum = numel(fileList);
datanum = 0;
for j = 1:filenum
    filename = fullfile(folder, fileList(j).name);
    data = importdata(filename);
    for i = 1:length(data)
        str = data(i,:);
        pattern = '[-+]?\d*\.?\d*'; % 正则表达式，匹配一个或多个数字
        matches = regexp(str, pattern, 'match'); % 查找所有匹配的数字
        a = cell2table(matches);
        b = table2array(a);
        for x = 1:length(b)
            c = cell2mat(b(x));
            d(x) = str2double(c);
        end
        T(i + datanum) = d(3)*1000 + d(2)*1000*60 + d(1)*1000*60*60;
        if (i == 1 && j ==1)
            TT = T(1);
        end
        T(i + datanum) = T(i + datanum) - TT;
        X(i + datanum) = d(6)*100;
        Y(i + datanum) = d(7)*100;
        V(i + datanum) = d(8)*100;
        A(i + datanum) = d(9);
        R(i + datanum) = d(10)*100;
        ID(i + datanum) = d(11);
    end
    datanum = datanum + length(data);
end
T = T/1000;

subplot 221
plot(T,R,".");
xlabel("时间  s")
ylabel("距离 cm")
subtitle("距离 cm")
subplot 222
plot3(T,X,Y,".");
xlabel("时间  s")
ylabel("X轴距离 cm")
zlabel("Y轴距离 cm")
subtitle("方位距离 cm")
subplot 223
plot(T,V,".");
xlabel("时间  s")
ylabel("速度 cm/s")
subtitle("速度 cm/s")
subplot 224
plot(T,A,".");
xlabel("时间  s")
ylabel("角度 度")
subtitle("水平角 度")
figure
idnum = 0;
newclassflag = 0;
recordflag = 0;
idclass = 1000 * ones(1,200);
idcladd_cnt = ones(1,200);
for i = 1:length(T)
    for j = 1:length(idclass)
        if(ID(i) == idclass(j))
            Tid(j,idcladd_cnt(j)) = T(i);
            Xid(j,idcladd_cnt(j)) = X(i);
            Yid(j,idcladd_cnt(j)) = Y(i);
            Vid(j,idcladd_cnt(j)) = V(i);
            Aid(j,idcladd_cnt(j)) = A(i);
            Rid(j,idcladd_cnt(j)) = R(i);
            idcladd_cnt(j) = idcladd_cnt(j) + 1; 
            recordflag = 1;
        end
    end
    if recordflag == 0
        idnum = idnum + 1;
        idclass(idnum) = ID(i);
        Tid(idnum,idcladd_cnt(idnum)) = T(i);
        Xid(idnum,idcladd_cnt(idnum)) = X(i);
        Yid(idnum,idcladd_cnt(idnum)) = Y(i);
        Vid(idnum,idcladd_cnt(idnum)) = V(i);
        Aid(idnum,idcladd_cnt(idnum)) = A(i);
        Rid(idnum,idcladd_cnt(idnum)) = R(i);
        idcladd_cnt(idnum) = idcladd_cnt(idnum) + 1; 
    end
    recordflag = 0;
end

% 定义颜色循环数组
colors = ['r', 'g', 'b', 'y', 'm', 'c','k'];
colors = [colors,colors,colors,colors,colors,colors,colors,colors,colors];
subplot 221
for i = 1:idnum
    color = colors(i);
    plot(Tid(i,:),Rid(i,:),".",'Color', color);
    hold on;
end
xlabel("时间  s")
ylabel("距离 cm")
subtitle("距离 cm")
subplot 222
for i = 1:idnum
    color = colors(i);
    plot3(Tid(i,:),Xid(i,:),Yid(i,:),".",'Color', color);
    hold on;
end
xlabel("时间  s")
ylabel("X轴距离 cm")
zlabel("Y轴距离 cm")
subtitle("方位距离 cm")
subplot 223
for i = 1:idnum
    color = colors(i);
    plot(Tid(i,:),Vid(i,:),".",'Color', color);
    hold on;
end
xlabel("时间  s")
ylabel("速度 cm/s")
subtitle("速度 cm/s")
subplot 224
for i = 1:idnum
    color = colors(i);
    plot(Tid(i,:),Aid(i,:),".",'Color', color);
    hold on;
end
xlabel("时间  s")
ylabel("角度 度")
subtitle("水平角 度")

% point,k,count,xlab,ylab,tit
figure
subplot 211
for i = 1:idnum
    color = colors(i);
    plot(Tid(i,:),Rid(i,:),".",'Color', color);
    hold on;
end
xlabel("时间  s")
ylabel("距离 cm")
subtitle("距离 cm")
subplot 212
fun_k_means([T;R]',k,count,"时间 s","距离 cm","k均值聚类",0);
% fun_dbscan([T;R]',epsilon,minpts,"时间 s","距离 cm",1);